#ifndef _EXTENDED_KALMAN_FILTER_
#define _EXTENDED_KALMAN_FILTER_
#include <eigen3/Eigen/Eigen>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

/**
 * @brief Extended Kalman Filter class
 * @param T        scaler type
 * @param System   system class to be estimated
 */

template<class System>
class ExtendedKalmanFilterX {
    typedef float T;
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;
    typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
public:
    /**
   * @brief constructor
   * @param system               system to be estimated
   * @param state_dim            state vector dimension (px,vx,py,vy)
   * @param input_dim            input vector dimension (None)
   * @param measurement_dim      measurement vector dimension (Radar: 2-dims: rho and angle)
   * @param process_noise        process noise covariance (state_dim x state_dim)
   * @param measurement_noise    measurement noise covariance (measurement_dim x measuremend_dim)
   * @param mean                 initial mean
   * @param cov                  initial covariance
   */
    ExtendedKalmanFilterX(const System &system_,int state_dim_, int input_dim_, int measurement_dim_,
                          const MatrixXt& process_noise, const MatrixXt& measurement_noise,
                          const VectorXt& mean_, const MatrixXt& cov_)
        : state_dim(state_dim_),
          input_dim(input_dim_),
          measurement_dim(measurement_dim_),
          mean(mean_),
          cov(cov_),
          system(system_),
          process_noise(process_noise),
          measurement_noise(measurement_noise)
    {
    }

    /**
    * @brief predict
    * @param control  input vector
    */
    void predict()
    {
        VectorXt mean_pred = system.f(mean);
        MatrixXt cov_pred = system.F(mean)*cov*system.F(mean).transpose() + process_noise;
        mean = mean_pred;
        cov = cov_pred;
    }
    //!< TODO
    void predict(const VectorXt& control)
    {
    }

    /**
    * @brief correct
    * @param measurement  measurement vector
    */
    void correct(const VectorXt& measurement)
    {
        MatrixXt JacobianH = system.H(mean);
        MatrixXt JacobianHT = JacobianH.transpose();
        kalman_gain = cov * JacobianHT*
                (JacobianH * cov * JacobianHT + measurement_noise).inverse();


        VectorXt predMeasure = system.h(mean);
        mean = mean + kalman_gain*(measurement - predMeasure);

        MatrixXt identity = Eigen::MatrixXf::Identity(state_dim,state_dim);
        cov = (identity - kalman_gain*JacobianH)*cov;
    }


    /*			getter			*/
    const VectorXt& getMean() const { return mean; }
    const MatrixXt& getCov() const { return cov; }

    System& getSystem() { return system; }
    const System& getSystem() const { return system; }
    const MatrixXt& getProcessNoiseCov() const { return process_noise; }
    const MatrixXt& getMeasurementNoiseCov() const { return measurement_noise; }
    const MatrixXt& getKalmanGain() const { return kalman_gain; }

    /*			setter			*/
    ExtendedKalmanFilterX& setMean(const VectorXt& m) { mean = m;	return *this; }
    ExtendedKalmanFilterX& setCov(const MatrixXt& s) { cov = s;		return *this; }

    ExtendedKalmanFilterX& setProcessNoiseCov(const MatrixXt& p) { process_noise = p;		return *this; }
    ExtendedKalmanFilterX& setMeasurementNoiseCov(const MatrixXt& m) { measurement_noise = m;	return *this; }


EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    private:
        const int state_dim;
    const int input_dim;
    const int measurement_dim;

public:
    VectorXt mean;
    MatrixXt cov;

    System system;
    MatrixXt process_noise;     //
    MatrixXt measurement_noise;	//
    MatrixXt kalman_gain;
};

#endif
